package com.awesumgames.awesum;

import static java.lang.Math.sqrt;
import static java.lang.Math.sin;
import static java.lang.Math.cos;
import static java.lang.Math.PI;

public final class TestVector {

	public float x, y, z;
	public static float degToRad = (float)(PI / 180f);
	public static float radToDeg = (float)(180f / PI);
	private static final int X = 0, Y = 1, Z = 2;
	
	public TestVector() {
		x = y = z = 0f;
	}
	public TestVector(TestVector v2) {
		x = v2.x;
		y = v2.y;
		z = v2.z;
	}
	public TestVector(float x, float y, float z) {
		this.x = x; this.y = y; this.z = z;
	}
	
	public String toString() {
		//return "(" + String.valueOf(x) + ", "
		return String.format("(%f, %f, %f)", x, y, z);
	}
	
	public TestVector copy() {
		return new TestVector(this);
	}

	public TestVector add(TestVector v2) {
		x += v2.x; y += v2.y; z += v2.z;
		return this;
	}
	public TestVector add(float s) {
		x += s; y += s; z += s;
		return this;
	}
	public TestVector subtract(TestVector v2) {
		//return new TestVector(x - v2.x, y - v2.y, z - v2.z);
		x -= v2.x; y -= v2.y; z -= v2.z;
		return this;
	}
	public TestVector multiply(float s) { //Scalar multiply
		//return new TestVector(x * s, y * s, z * s);
		x *= s; y *= s; z *= s;
		return this;
	}
	public TestVector multiply(TestVector v2) { //Scalar multiply individual components
		//return new TestVector(x * v2.x, y * v2.y, z * v2.z);
		x *= v2.x; y *= v2.y; z *= v2.z;
		return this;
	}
	public TestVector divide(float s) { //Scalar divide
		//return new TestVector(x / s, y / s, z / s);
		x /= s; y /= s; z /= s;
		return this;
	}
	public TestVector divide(TestVector v2) { //Scalar divide individual components
		//return new TestVector(x / v2.x, y / v2.y, z / v2.z);
		x /= v2.x; y /= v2.y; z /= v2.z;
		return this;
	}
	public TestVector mod(float s) { //Scalar modulo
		//return new TestVector(x % s, y % s, z % s);
		x %= s; y %= s; z %= s;
		return this;
	}
	public TestVector negate() {
		//return new TestVector(-x, -y, -z);
		return multiply(-1);
	}
	public Boolean equals(TestVector v2) {
		return x == v2.x && y == v2.y && z == v2.z;
	}
	public float magnitude() {
		return (float)sqrt(magnitudeSq());
	}
	public float magnitudeSq() {
		return x*x + y*y + z*z;
	}
	
	public void normalize() {
		float m = magnitude();
		divide(m);
	}
	public TestVector normal() {
		float m = magnitude();
		return new TestVector(x / m, y / m, z / m);
	}
	public float dot(TestVector v2) {
		return x*v2.x + y*v2.y + z*v2.z;
	}
	public TestVector cross(TestVector v2) {
		return new TestVector((float)(y*v2.z - z*v2.y), (float)(z*v2.x - x*v2.z), (float)(x*v2.y - y*v2.x));
	}
	public float distFrom(TestVector v2) {
		return (float)sqrt(distSqFrom(v2));
	}
	public float distSqFrom(TestVector v2) {
		double dx = x-v2.x, dy = y-v2.y, dz = z-v2.z;
		return (float)sqrt(dx*dx + dy*dy + dz*dz);
	}
	
	
	public TestVector rotateX(float a) {
		a *= degToRad;
		
		double ny = cos(a) * y + sin(a) * z;
		double nz = -sin(a) * y + cos(a) * z;
		
		y = (float)ny;
		z = (float)nz;
		return this;
	}

	public TestVector rotateY(float a) {
		a *= degToRad;
		
		double nx = cos(a) * x - sin(a) * z;
		double nz = sin(a) * x + cos(a) * z;
		
		x = (float)nx;
		z = (float)nz;
		return this;
	}

	public TestVector rotateZ(float a) {
		a *= degToRad;
		
		double nx = cos(a) * x + sin(a) * y;
		double ny = -sin(a) * x + cos(a) * y;
		
		x = (float)nx;
		y = (float)ny;
		return this;
	}

	public TestVector rotate(TestVector v2) { //This might encounter a gimbal lock problem
		if (v2.x != 0f) rotateX(v2.x);
		if (v2.y != 0f) rotateY(v2.y);
		if (v2.z != 0f) rotateZ(v2.z);
		return this;
	}
}
